#include <rtthread.h>

#include <stdio.h>

#include <dshot.h>
#include <copter.h>

static void motorThread(void *p)
{
    uint16_t val[4] = {0};
    NXMFC::dshot ds;

    if (!ds.init(":dev/pwm0"))
    {
        printf("init dshot fail\n");
        return;
    }

    rt_thread_mdelay(50);

    for (int i = 0; i < 2000; i++)
    {
        ds.output(val);
        rt_thread_mdelay(2);
    }

    while (1)
    {
        if (gCopter.armed())
        {
            val[0] = 200 + gCopter.throttle() * 1800;
            val[1] = 200 + gCopter.throttle() * 1800;
            val[2] = 200 + gCopter.throttle() * 1800;
            val[3] = 200 + gCopter.throttle() * 1800;
        }
        else
        {
            val[0] = val[1] = val[2] = val[3] = 0;
        }

        ds.output(val);
        rt_thread_mdelay(2);
    }
}

int taskMotorInit(void)
{
    rt_thread_t t;
    int ret = -1;

    t = rt_thread_create("motor", motorThread, 0,
                         1024, 20, 5);
    if (t)
        ret = rt_thread_startup(t);

    return ret;
}
INIT_APP_EXPORT(taskMotorInit);
